//
// Created by ubuntu on 2024/1/3.
//

#ifndef LASER_LINE_FITTER_SPLIT_AND_MERGE_H
#define LASER_LINE_FITTER_SPLIT_AND_MERGE_H

#include "line.h"

namespace line_extraction
{
class SplitAndMerge
{

public:
    // Constructor / destructor
    SplitAndMerge();

    ~SplitAndMerge();

    // Run
    void extractLines(std::vector<Line> &);

    Params params_;
    CSdata cs_data_;     // 数据形势
    Rangedata range_data_;  // 数据流
private:
    // Data structures
    // Indices after filtering
    std::vector<unsigned int> filtered_indices_;
    // Line data
    std::vector<Line> lines_;

    // Methods
    double chiSquared(const Eigen::Vector2d &, const Eigen::Matrix2d &,
                      const Eigen::Matrix2d &);

    double distBetweenPoints(unsigned int index_1, unsigned int index_2);

    void filterCloseAndFarPoints();

    void filterOutlierPoints();

    void filterLines();

    void mergeLines();

    void split(const std::vector<unsigned int> &);

};
}


#endif //LASER_LINE_FITTER_SPLIT_AND_MERGE_H
